/**
 ==== 定义全局配置参数 ====

 ==== 全局配置参数结束 ====
 */

// ==== 程序代码开始 ====
int t; //计时器

Foot L1;
Foot L2;
Foot L3;
Foot R1;
Foot R2;
Foot R3;

int step; //冲程
IMyTextPanel LCD;
IMyShipController Cockpit;
string CMD = "stop";
void Main(string arguments){
	if(L1 == null){init();return;}

	Echo("Program Running ...");

	t ++;

	if(arguments != ""){
		CMD = arguments;
		if(arguments == "jump") step = 0;
	}

	if(CMD == "stop"){
		L1.MotionStop(); L2.MotionStop(); L3.MotionStop(); R1.MotionStop(); R2.MotionStop(); R3.MotionStop();
	}else if(CMD == "hold"){
		L1.AttitudeInit(); L2.AttitudeInit(); L3.AttitudeInit(); R1.AttitudeInit(); R2.AttitudeInit(); R3.AttitudeInit();
	}else if(CMD == "run"){
		run();
	}else if(CMD == "left"){
		TurnLeft();
	}else if(CMD == "right"){
		TurnRight();
	}else if(CMD == "curlup"){
		L1.TelescopicUp(); L2.TelescopicUp(); L3.TelescopicUp(); R1.TelescopicUp(); R2.TelescopicUp(); R3.TelescopicUp();
	}else if(CMD == "stretch"){
		L1.TelescopicDown(); L2.TelescopicDown(); L3.TelescopicDown(); R1.TelescopicDown(); R2.TelescopicDown(); R3.TelescopicDown();
	}else if(CMD == "jump"){
		Jump(45);
	}

	LCD = GridTerminalSystem.GetBlockWithName("LCD") as IMyTextPanel;
	if(LCD != null){
		LCD.ShowPublicTextOnScreen();
		string str = "1:停止运动，2:六足回正\n3:前进，4:左转，5:右转\n6:蜷缩，7:展开，8:跳起" + "\n";
		str += "当前指令：" + CMD + " 冲程：" + step; 
		LCD.WritePublicText(str);
	}

	BalanceBody();
}

//平衡身体
void BalanceBody(){
	Vector3D myDown = Cockpit.WorldMatrix.Down;
	Vector3D gravity = Cockpit.GetNaturalGravity();
	Echo(gravity.ToString());
}

//向前奔跑
void run(){
	if(step == 0){
		bool a = L2.MoveBack(30);
		bool b = R1.MoveBack(30);
		bool c = R3.MoveBack(30);
		bool d = L1.RiseUp(30) && L1.MoveForward(30);
		bool e = L3.RiseUp(30) && L3.MoveForward(30);
		bool f = R2.RiseUp(30) && R2.MoveForward(30);
		if(a && b && c && d && e && f){
			step = 1;
		}
	}else if(step == 1){
		bool a = L1.RiseDown(30);
		bool b = L3.RiseDown(30);
		bool c = R2.RiseDown(30);
		if(a && b && c){
			step = 2;
		}
	}else if(step == 2){
		bool a = L1.MoveBack(30);
		bool b = L3.MoveBack(30);
		bool c = R2.MoveBack(30);
		bool d = L2.RiseUp(30) && L2.MoveForward(30);
		bool e = R1.RiseUp(30) && R1.MoveForward(30);
		bool f = R3.RiseUp(30) && R3.MoveForward(30);
		if(a && b && c && d && e && f){
			step = 3;
		}
	}else if(step == 3){
		bool a = L2.RiseDown(30);
		bool b = R1.RiseDown(30);
		bool c = R3.RiseDown(30);
		if(a && b && c){
			step = 0;
		}
	}
}

//左转
void TurnLeft(){
	if(step == 0){
		bool a = L2.MoveForward();
		bool b = R1.MoveBack();
		bool c = R3.MoveBack();
		bool d = L1.RiseUp() && L1.MoveBack();
		bool e = L3.RiseUp() && L3.MoveBack();
		bool f = R2.RiseUp() && R2.MoveForward();
		if(a && b && c && d && e && f){
			step = 1;
		}
	}else if(step == 1){
		bool a = L1.RiseDown();
		bool b = L3.RiseDown();
		bool c = R2.RiseDown();
		if(a && b && c){
			step = 2;
		}
	}else if(step == 2){
		bool a = L1.MoveForward();
		bool b = L3.MoveForward();
		bool c = R2.MoveBack();
		bool d = L2.RiseUp() && L2.MoveBack();
		bool e = R1.RiseUp() && R1.MoveForward();
		bool f = R3.RiseUp() && R3.MoveForward();
		if(a && b && c && d && e && f){
			step = 3;
		}
	}else if(step == 3){
		bool a = L2.RiseDown();
		bool b = R1.RiseDown();
		bool c = R3.RiseDown();
		if(a && b && c){
			step = 0;
		}
	}
}

//右转
void TurnRight(){
	if(step == 0){
		bool a = L2.MoveBack();
		bool b = R1.MoveForward();
		bool c = R3.MoveForward();
		bool d = L1.RiseUp() && L1.MoveForward();
		bool e = L3.RiseUp() && L3.MoveForward();
		bool f = R2.RiseUp() && R2.MoveBack();
		if(a && b && c && d && e && f){
			step = 1;
		}
	}else if(step == 1){
		bool a = L1.RiseDown();
		bool b = L3.RiseDown();
		bool c = R2.RiseDown();
		if(a && b && c){
			step = 2;
		}
	}else if(step == 2){
		bool a = L1.MoveBack();
		bool b = L3.MoveBack();
		bool c = R2.MoveForward();
		bool d = L2.RiseUp() && L2.MoveForward();
		bool e = R1.RiseUp() && R1.MoveBack();
		bool f = R3.RiseUp() && R3.MoveBack();
		if(a && b && c && d && e && f){
			step = 3;
		}
	}else if(step == 3){
		bool a = L2.RiseDown();
		bool b = R1.RiseDown();
		bool c = R3.RiseDown();
		if(a && b && c){
			step = 0;
		}
	}
}

//跳跃
void Jump(double ag){
	if(step == 0){
		bool a = L1.CenterDown(ag, 5);
		bool b = L2.CenterDown(ag, 5);
		bool c = L3.CenterDown(ag, 5);
		bool d = R1.CenterDown(ag, 5);
		bool e = R2.CenterDown(ag, 5);
		bool f = R3.CenterDown(ag, 5);
		if(a && b && c && d && e && f){
			step = 1;
		}
	}else if(step == 1){
		bool a = L1.CenterUp(ag, 60);
		bool b = L2.CenterUp(ag, 60);
		bool c = L3.CenterUp(ag, 60);
		bool d = R1.CenterUp(ag, 60);
		bool e = R2.CenterUp(ag, 60);
		bool f = R3.CenterUp(ag, 60);
		if(a && b && c && d && e && f){
			step = 2;
		}
	}else if(step == 2){
		bool a = L1.AttitudeInit();
		bool b = L2.AttitudeInit();
		bool c = L3.AttitudeInit();
		bool d = R1.AttitudeInit();
		bool e = R2.AttitudeInit();
		bool f = R3.AttitudeInit();
	}
}

void RunFast(){
	L2.Joint_X.setAngleTo(0);
	L2.Joint_Y1.setAngleTo(new Angle(50*L1.AxialX));
	L2.Joint_Y2.setAngleTo(new Angle(30*L2.AxialX));
	R2.Joint_X.setAngleTo(0);
	R2.Joint_Y1.setAngleTo(new Angle(50*R2.AxialX));
	R2.Joint_Y2.setAngleTo(new Angle(30*R2.AxialX));
	if(step == 0){
		bool a = L1.Rise(new Angle(-25)) && L1.MoveForward();
		bool b = R1.Rise(new Angle(-25)) && R1.MoveForward();
		if(a && b){
			step = 1;
		}
	}else if(step == 1){
		bool a = L3.Rise(new Angle(25));
		bool b = R3.Rise(new Angle(25));
		bool c = L3.MoveBack();
		bool d = R3.MoveBack();
		if(a && b && c && d){
			step = 2;
		}
	}else if(step == 2){
		bool a = L1.MoveBack();
		bool b = R1.MoveBack();
		bool c = L1.Rise(new Angle(25));
		bool d = R1.Rise(new Angle(25));
		if(a && b && c && d){
			step = 3;
		}
	}else if(step == 3){
		bool a = L3.Rise(new Angle(-25));
		bool b = R3.Rise(new Angle(-25));
		bool c = L3.MoveForward();
		bool d = R3.MoveForward();
		if(a && b && c && d){
			step = 0;
		}
	}
}

void init(){
	Cockpit = GridTerminalSystem.GetBlockWithName("Cockpit") as IMyShipController;
	L1 = new Foot(
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R1", "[FT]R_X") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R1", "[FT]R_Y1") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R1", "[FT]R_Y2") as IMyMotorStator)
	);
	L2 = new Foot(
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R2", "[FT]R_X") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R2", "[FT]R_Y1") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R2", "[FT]R_Y2") as IMyMotorStator)
	);
	L3 = new Foot(
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R3", "[FT]R_X") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R3", "[FT]R_Y1") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_R3", "[FT]R_Y2") as IMyMotorStator)
	);
	R1 = new Foot(
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L1", "[FT]R_X") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L1", "[FT]R_Y1") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L1", "[FT]R_Y2") as IMyMotorStator)
	);
	R2 = new Foot(
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L2", "[FT]R_X") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L2", "[FT]R_Y1") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L2", "[FT]R_Y2") as IMyMotorStator)
	);
	R3 = new Foot(
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L3", "[FT]R_X") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L3", "[FT]R_Y1") as IMyMotorStator),
		new Joint(getGroupBlocksByBlockName("[FT]Foot_L3", "[FT]R_Y2") as IMyMotorStator)
	);
	R1.AxialX = R2.AxialX = R3.AxialX = -1;
}

//从分组中获取方块的辅助方法
IMyTerminalBlock getGroupBlocksByBlockName(string group_name, string block_name){
	List<IMyBlockGroup> groups = new List<IMyBlockGroup>();
	GridTerminalSystem.GetBlockGroups(groups);
	foreach(IMyBlockGroup g in groups){
		if(g.Name == group_name){
			List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
			g.GetBlocks(blocks);
			foreach(IMyTerminalBlock b in blocks){
				if(b.CustomName == block_name){
					return b;
				}
			}
		}
	}
	return null;
}

// ==== 足类 ====
// 3关节，含1个水平关节和2个垂直关节
public class Foot
{
	public Joint Joint_X; //水平关节
	public Joint Joint_Y1; //垂直关节1
	public Joint Joint_Y2; //垂直关节2

	public int AxialX = 1; //左右侧轴向，-1左侧，1右侧
	public double JointMoveVelocity = 30; //关节转速

	public Foot(Joint x, Joint y1, Joint y2){
		this.Joint_X = x;
		this.Joint_X.accuracy_ratio = 12*Math.PI/180;
		// this.Joint_X.MinAngle = 10*Math.PI/180;
		// this.Joint_X.MaxAngle = -10*Math.PI/180;
		this.Joint_Y1 = y1;
		this.Joint_Y1.accuracy_ratio = 8*Math.PI/180;
		this.Joint_Y2 = y2;
	}

	// ==== 运动停止 ====
	public void MotionStop(){
		this.Joint_X.MotionStop();
		this.Joint_Y1.MotionStop();
		this.Joint_Y2.MotionStop();
	}
	// ==== 运动锁死 ====
	public void MotionLock(bool lockOn = true){
		this.Joint_X.MotionLock(lockOn);
		this.Joint_Y1.MotionLock(lockOn);
		this.Joint_Y2.MotionLock(lockOn);
	}

	// ==== 姿态还原 ====
	public bool AttitudeInit(double velocity = 0){
		bool mt_1 = this.Joint_X.setAngleTo(0, velocity == 0 ? JointMoveVelocity : velocity);
		bool mt_2 = this.Joint_Y1.setAngleTo(0, velocity == 0 ? JointMoveVelocity : velocity);
		bool mt_3 = this.Joint_Y2.setAngleTo(0, velocity == 0 ? JointMoveVelocity : velocity);
		return  mt_1 && mt_2 && mt_3;
	}

	// ==== 垂直运动 ====
	public bool Move(Angle angle, double velocity = 0){
		return this.Joint_X.setAngleTo(angle, velocity == 0 ? JointMoveVelocity : velocity);
	}
	public bool MoveForward(double velocity = 0){
		return this.Move(new Angle(22*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
	}
	public bool MoveCenter(double velocity = 0){
		return this.Move(new Angle(), velocity == 0 ? JointMoveVelocity : velocity);
	}
	public bool MoveBack(double velocity = 0){
		return this.Move(new Angle(-22*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
	}

	// ==== 水平运动 ====
	public bool Rise(Angle angle, double velocity = 0){
		bool mt_1 = this.Joint_Y1.setAngleTo(new Angle(angle.angle*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		bool mt_2 = this.Joint_Y2.setAngleTo(new Angle(-angle.angle*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		return mt_1 && mt_2;
	}
	public bool RiseUp(double velocity = 0){
		return this.Rise(new Angle(-20), velocity == 0 ? JointMoveVelocity : velocity);
	}
	public bool RiseCenter(double velocity = 0){
		return this.Rise(new Angle(), velocity == 0 ? JointMoveVelocity : velocity);
	}
	public bool RiseDown(double velocity = 0){
		return this.Rise(new Angle(20), velocity == 0 ? JointMoveVelocity : velocity);
	}

	// ==== 姿态运动 ====
	public bool Telescopic(Angle angle, double velocity = 0){
		bool a = this.Joint_Y1.setAngleTo(new Angle(angle.angle*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		bool b = this.Joint_Y2.setAngleTo(new Angle(), velocity == 0 ? JointMoveVelocity : velocity);
		return a && b;
	}
	public bool TelescopicUp(double velocity = 0){
		return this.Telescopic(new Angle(30), velocity == 0 ? JointMoveVelocity : velocity);
	}
	public bool TelescopicDown(double velocity = 0){
		return this.Telescopic(new Angle(-20), velocity == 0 ? JointMoveVelocity : velocity);
	}
	
	// ==== 升降重心 ====
	// input double ag 抬或降低的角度，传入正数即可
	public bool CenterUp(double ag, double velocity = 0){
		bool a = this.Joint_X.setAngleTo(new Angle(0), velocity == 0 ? JointMoveVelocity : velocity);
		bool b = this.Joint_Y1.setAngleTo(new Angle(ag*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		bool c = this.Joint_Y2.setAngleTo(new Angle(-ag*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		return a && b && c;
	}
	public bool CenterDown(double ag, double velocity = 0){
		bool a = this.Joint_X.setAngleTo(new Angle(0), velocity == 0 ? JointMoveVelocity : velocity);
		bool b = this.Joint_Y1.setAngleTo(new Angle(-ag*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		bool c = this.Joint_Y2.setAngleTo(new Angle(ag*this.AxialX), velocity == 0 ? JointMoveVelocity : velocity);
		return a && b && c;
	}
}

// ==== 关节类 ====
public class Joint
{
    public readonly string type; //类型（rotor转子、rotors双转子、rotors_2同向双转子、piston活塞）
    public readonly IMyMotorStator Rotor; //核心转子，必存在，转子转速RPM是-30到30
    public readonly IMyMotorStator RotorHelper; //辅助转子，仅当双转子型存在
    public readonly IMyPistonBase Piston; //活塞，仅当活塞型关节存在，活塞速度是-5到5
    public readonly float initAngle; //初始角度，初始角度取决于关节被实例化时的核心转子角度

	//当前角度，本属性随方法自动更新
	private float current_angle;
	public float CurrentAngle{
		get {return this.current_angle;}
	}

	public string debug = "normal";
	public double MaxAngle = 2*Math.PI; //最大转动弧度
	public double MinAngle = 0; //最小转动弧度
    public int PosNeg = 1; //执行正反系数

	public double accuracy_damping = 30*Math.PI/180; //转动阻尼系数（弧度），当转动抵达阻尼角度内转子从满速转动变为精确转动，转动惯性越大系数应该越大。
	public double accuracy_ratio = 3*Math.PI/180; //转动系数精度（弧度），当转子转动抵达精度范围内，判定转动完成
    
    //构造方法
    //单转子关节
    public Joint(IMyMotorStator rotor){
        this.type = "rotor";
        this.Rotor = rotor;
        this.initAngle = this.Rotor.Angle;
        initBlock();
    }
    //双转子关节
    public Joint(IMyMotorStator main_rotor, IMyMotorStator help_rotor, bool is_same_rotor = false){
        this.type = "rotors";
		if(is_same_rotor){
			this.type = "rotors_2";
		}
        this.Rotor = main_rotor;
        this.RotorHelper = help_rotor;
        this.initAngle = this.Rotor.Angle;
        initBlock();
    }
    //活塞型关节
    public Joint(IMyMotorStator rotor, IMyPistonBase piston){
        this.type = "piston";
        this.Rotor = rotor;
        this.Piston = piston;
        this.initAngle = this.Rotor.Angle;
        initBlock();
    }
    //初始化功能块设置
    private void initBlock(){
		this.current_angle = this.Rotor.Angle;
		this.MotionLock(false);
    }

	//运动事件周期
	private void beforeMotion(){
		this.current_angle = this.Rotor.Angle;
	}
	private void afterMoton(){

	}
	
	/*
		==== 设置关节转到某角度 ====
		input x 目标角度，弧度，超过后会被规范到0-2π之间
		input velocity 固定速度，RPM，设为0采用自动计算
		input move_type 移动方式，默认0就近，1顺时针，-1逆时针
	 */
	public bool setAngleTo(float x, double velocity = 0, int move_type = 0){
		this.beforeMotion();

		//规范输入范围
		if(x > 2*Math.PI) x = (float)(2*Math.PI);
		else if(x < 0) x = 0;

		//计算转动路径路径
		if(move_type == 0){
			if(Math.Abs(x - this.current_angle) <= 0.5*Math.PI){
				//如果当前与目标点直接弧长小于半周长
				move_type = x - this.current_angle > 0 ? 1 : -1;
			}else{
				move_type = x - this.current_angle > 0 ? -1 : 1;
			}
		}

		//计算结果输出量
		float res = 0; //输出范围-1~1
		if(Math.Abs(x - this.current_angle) > this.accuracy_damping){
			res = move_type;
		}else{
			res = Math.Abs(x - this.current_angle)*move_type/(2*(float)Math.PI);
		}
		res = res*this.PosNeg; //考虑正反系数

		//考虑角度限制
		if(this.MaxAngle > this.MinAngle){
			if(this.current_angle < this.MinAngle){
				res = 0.3f;
			}else if(this.current_angle > this.MaxAngle){
				res = -0.3f;
			}
		}else{
			if(this.current_angle > this.MinAngle){
				res = -0.3f;
			}else if(this.current_angle < this.MaxAngle){
				res = 0.5f;
			}
		}
		

		//根据关节类型处理结果
		if(velocity == 0){
			if(this.type == "rotor"){
				this.Rotor.TargetVelocityRPM = res*30;
			}else if(this.type == "rotors"){
				this.Rotor.TargetVelocityRPM = res*30;
				this.RotorHelper.TargetVelocityRPM = -res*30;
			}else if(this.type == "piston"){
				this.Piston.Velocity = res*5;
			}
		}else{
			if(this.type == "rotor"){
				this.Rotor.TargetVelocityRPM = res*(float)velocity;
			}else if(this.type == "rotors"){
				this.Rotor.TargetVelocityRPM = res*(float)velocity;
				this.RotorHelper.TargetVelocityRPM = -res*(float)velocity;
			}else if(this.type == "piston"){
				this.Piston.Velocity = res*(float)velocity;
			}else if(this.type == "rotors_2"){
				//同向双转子
				this.Rotor.TargetVelocityRPM = res*(float)velocity;
				this.RotorHelper.TargetVelocityRPM = res*(float)velocity;
			}
		}
		
		
		this.afterMoton();
		return Math.Abs(this.current_angle - x) < this.accuracy_ratio;
	}
	public bool setAngleTo(double x, double velocity = 0, int move_type = 0){
		return this.setAngleTo((float)x, velocity, move_type);
	}
	public bool setAngleTo(Angle x, double velocity = 0, int move_type = 0){
		return this.setAngleTo((float)x.rad, velocity, move_type);
	}

	// ==== 运动停止 ====
	public void MotionStop(){
		this.Rotor.TargetVelocityRPM = 0;
		if(this.RotorHelper != null) this.RotorHelper.TargetVelocityRPM = 0;
		if(this.Piston != null) this.Piston.Velocity = 0;
	}

	// ==== 转子锁死 ====
	public void MotionLock(bool lockOn = true){
		this.Rotor.RotorLock = lockOn;
		if(this.RotorHelper != null) this.RotorHelper.RotorLock = lockOn;
	}
}

// ==== 角度运算类 ====
public class Angle
{
	public double angle; //角度
	public double rad{
		get{
			return Angle.getRad(this.angle);
		}
	} //弧度

	public Angle(double value = 0, bool is_rad = false){
		value = is_rad ? Angle.getAngle(value) : value;
		if(Math.Abs(value) > 360){
			value = value%360;
		}
		if(value < 0){
			value = 360 + value;
		}
		this.angle = is_rad ? Angle.getAngle(value) : value;
	}

	//定义加法运算
	public Angle Add(double value, bool is_rad = false){
		value = is_rad ? Angle.getAngle(value) : value;
		this.angle = (this.angle + value) % 360 > 0 ? (this.angle + value) % 360 : 360 + ((this.angle + value) % 360);
		return this;
	}
	public static Angle operator+(Angle ag_1, Angle ag_2){
		return new Angle().Add(ag_1.angle).Add(ag_2.angle);
	}

	// 根据角度获取弧度
	public static double getRad(double angle){
		return angle*Math.PI/180;
	}
	// 根据弧度获取角度
	public static double getAngle(double rad){
		return rad*180/Math.PI;
	}
}


// ==== 调起保存后自动运行 ====
Program(){
	Runtime.UpdateFrequency = UpdateFrequency.Update1;
}